/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package roombaComm;

import java.io.IOException;
import java.util.Scanner;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.*;
import java.util.logging.*;
import java.io.*;
import gnu.io.*;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

/**
 *
 * @author i8913587
 */
public class RoombaUSAL {

    boolean debug = true;
    static int defaultOdometryUpdateTime; //Asignar valor por defecto
    static int defaultSpeed;
    
    int odometryUpdateTime; //Configurable por el usuario
    //Unidad Milisegundos
    
    float leftEncoderConversion;
    float rightEncoderConversion;
    float x = 0, y = 0, Theta = 0;
    
    //Cosas nuevas
    HiloOdometria odometria;
    Thread hiloOdometria;
    static int WHEEL_DISTANCE = 280;
    
    //Comunicacion Puerto serie
    private Logger logger;

    private int port_speed;
    private String port_name = null;
    private SerialPort serial_port = null;
    private InputStream serial_input_stream = null;
    private OutputStream serial_output_stream = null;
    
    private boolean connected = false;
    
    static int BumpsAndWheelDrops=0;
    static int Wall=1;
    static int CliffLeft=2;
    static int CliffFrontLeft=3;
    static int CliffFrontRight=4;
    static int CliffRight=5;
    static int VirtualWall=6;
    static int WheelOvercurrents=7;
    static int DirtDetect=8;
    static int UnusedByte=9;
    
    /** Group Sensors 2 **/
    static int IRCode=0;
    static int Buttons=1;
    //static int Distance=2;
    //static int Angle=4;
    
    /** Group Sensors 3 **/
    static int ChargingState=0;
    static int Voltage=1;
    static int Current=3;
    static int Temperature=5;
    static int BatteryCharge=6;
    static int BatteryCapacity=8;
    
    /** Group Sensors 4 **/
    static int WallSignal=0;
    static int CliffLeftSignal=2;
    static int CliffFrontLeftSignal=4;
    static int CliffFrontRightSignal=6;
    static int CliffRightSignal=8;
    static int ChargerAvailable=13;
    
    /** Group Sensors 5**/
    static int OpenInterfaceMode=0;
    
    /** Group Sensors 6**/
    static int LightBumpLeftSignal=0;
    static int LightBumpFrontLeftSignal=2;
    static int LightBumpFrontRightSignal=8;
    static int LightBumpRightSignal=10;
 
    char[] sensorsGroup1=new char[10];
    char[] sensorsGroup2=new char[6];
    char[] sensorsGroup3=new char[10];
    char[] sensorsGroup4=new char[14];
    char[] sensorsGroup5=new char[12];
    char[] sensorsGroup106=new char[12];

    public static final int GRUPO_SENSORES_1 = 0;
    public static final int GRUPO_SENSORES_2 = 1;
    public static final int GRUPO_SENSORES_3 = 2;
    public static final int GRUPO_SENSORES_4 = 3;
    public static final int GRUPO_SENSORES_5 = 4;
    public static final int GRUPO_SENSORES_106 = 5;
    public static final int ODOMETRIA = -1;
    
    public static final int GRUPO_SENSORES_1_ID = 1;
    public static final int GRUPO_SENSORES_2_ID = 2;
    public static final int GRUPO_SENSORES_3_ID = 3;
    public static final int GRUPO_SENSORES_4_ID = 4;
    public static final int GRUPO_SENSORES_5_ID = 5;
    public static final int GRUPO_SENSORES_106_ID = 106;
    
    public static final int GRUPO_SENSORES_1_TAM = 10;
    public static final int GRUPO_SENSORES_2_TAM = 6;
    public static final int GRUPO_SENSORES_3_TAM = 10;
    public static final int GRUPO_SENSORES_4_TAM = 14;
    public static final int GRUPO_SENSORES_5_TAM = 12;
    public static final int GRUPO_SENSORES_106_TAM = 12;
    
    public static final int SEND_LEDS = 139;
    public static final int SEND_SENSORS = 142;
    public static final int SEND_DIGIT_LEDS = 164;
    
    private byte datosOdometria[];
    
    private HiloSensores hilos[];
    
    private final Object synchro;
    
        ///Comandos accion
    public static final int DEFAULTSPEED = 400;
    int speed = DEFAULTSPEED;

    // Macros propias del Roomba
    public static int DRIVE       = 137;
    public static int MOTORS      = 138;
    public static int DRIVEWHEELS = 145;

    public byte sensores [];
        
    //===============================================================
    public RoombaUSAL(boolean debug, int port_speed) 
    //===============================================================
    {
        this.port_speed = port_speed;

        // InicializaciÃ³n del logger
        logger = Logger.getLogger(this.getClass().getCanonicalName());

        if (debug) 
        {
            logger.setLevel(Level.INFO);
        } 
        else 
        {
            logger.setLevel(Level.WARNING);
        }
        
        //logger.info("Inicializando RoombaUSAL " + this.VERSION);
        logger.info("RoombaUSAL inicializada");
        
        this.sensorsGroup1 = new char[GRUPO_SENSORES_1_TAM];
	this.sensorsGroup2 = new char[GRUPO_SENSORES_2_TAM];
	this.sensorsGroup3 = new char[GRUPO_SENSORES_3_TAM];
	this.sensorsGroup4 = new char[GRUPO_SENSORES_4_TAM];
	this.sensorsGroup5 = new char[GRUPO_SENSORES_5_TAM];
	this.sensorsGroup106 = new char[GRUPO_SENSORES_106_TAM];
    
	// Hilos no creados por defecto
	this.hilos = new HiloSensores[6];
	for (int i = 0; i < this.hilos.length; i++) 
        {
            this.hilos[i] = null;
	}
        
	synchro = new Object();
        
    }
    
    /* MÃ©todos estÃ¡ticos */
    public static short toShort(byte hi, byte lo) {
        ByteBuffer bb = ByteBuffer.allocate(2);
        bb.order(ByteOrder.LITTLE_ENDIAN);
        bb.put(hi);
        bb.put(lo);
        return bb.getShort(0);
        
    }
    
    public static int toUnsignedShort(byte hi, byte lo) {
        short s = toShort(hi, lo);
        return s >= 0 ? s : 0x10000 + s; 
    }
    
    public static byte[] intToByteArray(int value) {
        return new byte[] {
                (byte)(value >>> 24),
                (byte)(value >>> 16),
                (byte)(value >>> 8),
                (byte)value};
    }
    
    /* MÃ©todos privados */
    private void setRootothSpeed(int speed) 
    {
        String speedStr = null;
        switch(speed) {
            case 19200:
                speedStr = "19";
                break;
            case 57600:
                speedStr = "57";
                break;
            case 115200:
                speedStr = "11";
                break;
            default:
                throw new UnsupportedOperationException("Velocidad no soportada");
        }
        send("$$$"); 
        read(4);
        
        send("SU," + speedStr + "\n");
        read(4);
        
        send("---\n");
        read(4);
        
        try {Thread.sleep(500);} catch(Exception e) {}
    }

    /* MÃ©todos pÃºblicos */
    @SuppressWarnings("unchecked")
    public String[] listPoDTR() {
        Enumeration<CommPortIdentifier> port_enum;
        
        LinkedList<String> poDTR = new LinkedList<String>();

		port_enum = CommPortIdentifier.getPortIdentifiers();
		
		CommPortIdentifier port_id;
		while (port_enum.hasMoreElements()) {
			port_id = (CommPortIdentifier) port_enum.nextElement();
			// SÃ³lo nos interesan los puertos serie
			if (port_id.getPortType() == CommPortIdentifier.PORT_SERIAL) {
				poDTR.add(port_id.getName());
			}
		}
		
        logger.info("Puertos encontrados: ");
        for (String port : poDTR) {
            logger.info(port);
        }
        
        // Es necesario convertir la lista enlazada a un array
        int reslen = poDTR.size();
        String[] result = new String[reslen];
        
        for(int i = 0; i<poDTR.size(); i++) {
            result[i] = poDTR.get(i);
        }
	
		return result;
    }
    
    public boolean connect(String portName) 
    {
        this.port_name = portName;
        connected = false;
	try 
        {
            CommPortIdentifier port_id = CommPortIdentifier.getPortIdentifier(portName);
            if (port_id.isCurrentlyOwned()) 
            {
                logger.warning("ERROR: Puerto en uso");
	    } 
            else 
            {
                // Abrimos el puerto
                serial_port = (SerialPort) port_id.open(this.getClass().getCanonicalName(), 5000);
		// Configuramos la conexiÃ³n serie
		serial_port.setSerialPortParams(this.port_speed, RoombaSerialConfig.DATABITS, RoombaSerialConfig.STOPBITS, RoombaSerialConfig.PARITY);
                // Creamos los streams de entrada y salida
		serial_input_stream = serial_port.getInputStream();
		serial_output_stream = serial_port.getOutputStream();
        	// Seteamos la velocidad de conexiÃ³n con el Roomba
                
                //*************************************************************
                //Connected = true tiene qur ir antes que setRootothSpeed
                logger.info("ConexiÃ³n establecida en el puerto " + portName);
		connected = true;
                this.setRootothSpeed(this.port_speed);
		
            }
	} 
        catch (NoSuchPortException e) 
        {
            logger.warning("Puerto inexistente: " + portName);
	} 
        catch (PortInUseException e) 
        {
            logger.warning("Puerto ocupado: " + portName);
	} 
        catch (Exception e) 
        {
            logger.warning("No se puede conectar: " + portName);
        }
        return connected;
    }
    
    public boolean connected() {
        return connected;
    }
    
    public void disconnect() {
		try {
			serial_output_stream.close();
			serial_port.close();
			connected = false;
			logger.info("ConexiÃ³n terminada");
		} catch (Exception e) {
		    logger.warning("Error en la desconexiÃ³n: " + e.getMessage());
		}
    }
    
    boolean send(byte[] msg) {    
        // Comprobamos la conexiÃ³n
		if (connected()) {
		    // Intentamos escribir
			try {
			    serial_output_stream.write(msg);
			} catch (IOException e) {
			    logger.warning("Error al enviar: " + e.getMessage());
			    return false;
			}
		} else {
		    logger.warning("Intento de escritura sin conexiÃ³n");
		    return false;
		}
		return true;
    }
    
    boolean send(int msg) {       
        return send(intToByteArray(msg));
    }
    
    boolean send(String msg) {
        return send(msg.getBytes());
    }
    
    
    byte[] read(int nbytes) {    
        byte[] buffer = new byte[nbytes];
        // Comprobamos la conexiÃ³n
		if (connected()) {
		    // Intentamos leer
			try {
			    int c;
			    int i = 0;
			    for(;;) {
			        // La lectura bloqueante no siempre funciona en RXTX;
			        // hay que emularla
			        while (-1 == (c = serial_input_stream.read())) { 
			            try {
			                Thread.sleep(50);
			            } catch (Exception e) {} 
			        }
			        buffer[i] = (byte) c;
			        i++;
			        if (i == nbytes) {
			            return buffer;   
			        }
			    }
			} catch (IOException e) {
			    logger.warning("Error al leer: " + e.getMessage());
			    return null;
			}
		} else {
		    logger.warning("Intento de lectura sin conexiÃ³n");
		    return null;
		}
    }

    
    public void start() {
        send(128);
    }
    
    public void control() {
        send(130);
    }
    
    public void safe() {
        send(131);
    }
    
    public void full() {
        send(132);
    }
    
    public void clean() {
        send(135);
    }
    
    public void max() {
        send(136);
    }
    
    public void spot() {
        send(134);
    }
    
    public void seekDock() {
        send(143);
    }   
    
    public void powerOff() {
        send(133);
    }
    
    public void wakeup() {
        send("$$$"); 
        read(4);
        
        send("S@,8080\n");
        read(4);

        send("S&,8080\n");
        read(4);

        send("S&,8000\n");
        read(4);
        
        send("---\n");
        read(4);
        
        try {Thread.sleep(500);} catch(Exception e) {}
    }
        
    public interface RoombaSerialConfig {
        public static int[] SPEED           = { 19200, 57600, 115200 };
        public static int[] PACKET_SIZE     = { 26, 10, 6, 10 };
        public static int DATABITS          = gnu.io.SerialPort.DATABITS_8;
        public static int PARITY            = gnu.io.SerialPort.PARITY_NONE;
        public static int STOPBITS          = gnu.io.SerialPort.STOPBITS_1;
        public static int FLOW_CONTROL      = gnu.io.SerialPort.FLOWCONTROL_NONE;
    }
    
    //////////////////////////FIN COM PUERTO SERIE
    
    ///SENSORES
   
    
    /** Sensores Grupo 1 */
    /**
     * The state of the bumper (0 = no bump, 1 = bump) and wheel drop sensors (0 = wheel raised, 1 = wheel
     * dropped) are sent as individual bits.
     * Range: 0 â€“ 15
     * bump
     * @return 
     */
     public boolean bump(){
        char paquete=sensorsGroup1[BumpsAndWheelDrops];
        
        if((paquete&0x01)!=0){
            return true;
        }else if((paquete&0x02)!=0){
            return true;
        }
                
        return false;
    }
     /**
      * The state of the bumper (0 = no bump, 1 = bump) and wheel drop sensors (0 = wheel raised, 1 = wheel
      * dropped) are sent as individual bits.
      * Range: 0 â€“ 15
      * bumpLeft
      * @return
      */
     public boolean bumpLeft(){
        char paquete=sensorsGroup1[BumpsAndWheelDrops];
         
        if((paquete&0x02)!=0){
            return true;
        }
                
        return false;
    }
    /**
     * The state of the bumper (0 = no bump, 1 = bump) and wheel drop sensors (0 = wheel raised, 1 = wheel
     * dropped) are sent as individual bits.
     * Range: 0 â€“ 15
     * bumpRight
     * @return
     */
    public boolean bumpRight(){
        char paquete=sensorsGroup1[BumpsAndWheelDrops];
        if((paquete&0x01)!=0){
            return true;
        }
                
        return false;
    }
    /**  public boolean wheelDropCenter(){} OBSOLETA */
    /**
     * The state of the bumper (0 = no bump, 1 = bump) and wheel drop sensors (0 = wheel raised, 1 = wheel
     * dropped) are sent as individual bits.
     * Range: 0 â€“ 15
     * wheelDropLeft
     * @return 
     */
    public boolean wheelDropLeft(){
        char paquete=sensorsGroup1[BumpsAndWheelDrops];
        if((paquete&0x08)!=0){
            return true;
        }
                
        return false;
    
    }
    /**
     * The state of the bumper (0 = no bump, 1 = bump) and wheel drop sensors (0 = wheel raised, 1 = wheel
     * dropped) are sent as individual bits.
     * Range: 0 â€“ 15
     * wheelDropRight
     * @return
     */
    public boolean wheelDropRight(){
        char paquete=sensorsGroup1[BumpsAndWheelDrops];
        if((paquete&0x04)!=0){
            return true;
        }
                
        return false;
    }
    /**
     * The state of the wall sensor is sent as a 1 bit value (0 = no wall, 1 = wall seen).
     * Range: 0 â€“ 1
     * @return
     */
    public boolean wall(){
        char paquete=sensorsGroup1[Wall];
        if((paquete&0x01)!=0){
            return true;
        }
                
        return false;
    }
    /**
     * The state of the cliff sensor on the front left of Roomba is sent as a 1 bit value (0 = no cliff, 1 = cliff).
     * Range: 0 â€“ 1
     * @return
     */
    public boolean cliffFrontLeft(){
        char paquete=sensorsGroup1[CliffLeft];
        if((paquete&0x01)!=0){
            return true;
        }
                
        return false;
    }
    /**
     * The state of the cliff sensor on the front right of Roomba is sent as a 1 bit value (0 = no cliff, 1 = cliff)
     * Range: 0 â€“ 1
     * @return
     */
    public boolean cliffFrontRight(){
        char paquete=sensorsGroup1[CliffFrontLeft];
        if((paquete&0x01)!=0){
            return true;
        }
                
        return false;
    }
    /**
     * The state of the cliff sensor on the left side of Roomba is sent as a 1 bit value (0 = no cliff, 1 = cliff).
     * Range: 0 â€“ 1
     * @return
     */
    public boolean cliffLeft(){
        char paquete=sensorsGroup1[CliffLeft];
        if((paquete&0x01)!=0){
            return true;
        }               
        return false;   
    }
    /**
     * The state of the cliff sensor on the right side of Roomba is sent as a 1 bit value (0 = no cliff, 1 = cliff)
     * Range: 0 â€“ 1
     * @return
     */
    public boolean cliffRight(){
        char paquete=sensorsGroup1[CliffRight];
        if((paquete&0x01)!=0){
            return true;
        }               
        return false; 
    }
    /**
     * The state of the virtual wall detector is sent as a 1 bit value (0 = no virtual wall detected, 1 = virtual wall
     * detected).
     * Range: 0 â€“ 1
     * @return
     */
    public boolean virtualWall(){
        char paquete=sensorsGroup1[VirtualWall];
        if((paquete&0x01)!=0){
            return true;
        }               
        return false; 
    }
    /**
     * The state of the four wheel overcurrent sensors are sent as individual bits (0 = no overcurrent, 1 =
     * overcurrent). There is no overcurrent sensor for the vacuum on Roomba 500.
     * Range: 0 â€“ 31
     * 
     * motorOvercurrentDriveLeft
     * @return
     */
    public boolean motorOvercurrentDriveLeft(){
        char paquete=sensorsGroup1[WheelOvercurrents];
        if((paquete&0x16)!=0){
            return true;
        }               
        return false; 
    }
    /**
     * The state of the four wheel overcurrent sensors are sent as individual bits (0 = no overcurrent, 1 =
     * overcurrent). There is no overcurrent sensor for the vacuum on Roomba 500.
     * Range: 0 â€“ 31
     * motorOvercurrentDriveRight
     * @return
     */
    public boolean motorOvercurrentDriveRight(){
        char paquete=sensorsGroup1[WheelOvercurrents];
        if((paquete&0x08)!=0){
            return true;
        }               
        return false;
    }
    /**
     * The state of the four wheel overcurrent sensors are sent as individual bits (0 = no overcurrent, 1 =
     * overcurrent). There is no overcurrent sensor for the vacuum on Roomba 500.
     * Range: 0 â€“ 31
     * motorOvercurrentMainBrush
     * @return
     */
    public boolean motorOvercurrentMainBrush(){
        char paquete=sensorsGroup1[WheelOvercurrents];
        if((paquete&0x04)!=0){
            return true;
        }               
        return false;
    }
    /**
     * The state of the four wheel overcurrent sensors are sent as individual bits (0 = no overcurrent, 1 =
     * overcurrent). There is no overcurrent sensor for the vacuum on Roomba 500.
     * Range: 0 â€“ 31
     * motorOvercurrentSideBrush
     * @return
     */
    public boolean motorOvercurrentSideBrush(){
        char paquete=sensorsGroup1[WheelOvercurrents];
        if((paquete&0x01)!=0){
            return true;
        }               
        return false;
    }
    /**
     * The state of the four wheel overcurrent sensors are sent as individual bits (0 = no overcurrent, 1 =
     * overcurrent). There is no overcurrent sensor for the vacuum on Roomba 500.
     * Range: 0 â€“ 31
     * motorOvercurrentVacuum
     * @return
     */
    public boolean motorOvercurrentVacuum(){
        char paquete=sensorsGroup1[WheelOvercurrents];
        /*if((paquete&0x01)!=0){
            return true;
        }*/               
        return false;
    }
    /**
     * The level of the dirt detect sensor.
     * Range: 0-255
     * @return
     */
    public int dirt(){
        char paquete=sensorsGroup1[DirtDetect];
        
        //Pasamos el paquete a byte, y lo devolvemos como valor entero (rango 0-255)
        byte b = (byte)paquete;
        
        //System.out.println("El valor es "+Integer.toBinaryString((int)b));
        return (int)b;
    }
    
    /** Sensores Grupo 2*/
    /**
     * 
     * @return 
     */
    public char getIRCode(){
        char paquete=sensorsGroup2[IRCode];
        
        //SegÃºn el prototipo, la funciÃ³n devuelve un unsigned char
        
        //System.out.println("El valor es "+Integer.toBinaryString((int)b));
        return paquete;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * ClockButton
     * @return
     */
    public boolean clockButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x128)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * ScheduleButton
     * @return
     */
    public boolean scheduleButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x64)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * DayButton
     * @return
     */
    public boolean dayButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x32)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * HourButton
     * @return
     */
    public boolean hourButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x16)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * MinuteButton
     * @return
     */
    public boolean minuteButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x08)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * DockButton
     * @return
     */
    public boolean dockButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x04)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * SpotButton
     * @return
     */
    public boolean spotButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x02)!=0){
            return true;
        } 
        return true;
    }
    /**
     * The state of the Roomba buttons are sent as individual bits (0 = button not pressed, 1 = button
     * pressed). The day, hour, minute, clock, and scheduling buttons that exist only on Roomba 560 and 570
     * will always return 0 on a robot without these buttons.
     * Range: 0 â€“ 255
     * CleanButton
     * @return
     */
    public boolean cleanButton(){
        char paquete=sensorsGroup2[Buttons];
        if((paquete&0x01)!=0){
            return true;
        } 
        return true;
    }
    
    /** Sensores Grupo 3 */
    /**
     * No aparece en el manual de referencia
     * @return 
     */
    public boolean updateInternalState(){
        
        //No aparece en el manual de referencia de la API
        
        return true;
    }
    /**
     * This code indicates Roombaâ€™s current charging state.
     * Range: 0 â€“ 5
     * @return
     */
    public int chargingState(){
        
        /*
        Code Charging State
        0 Not charging
        1 Reconditioning Charging
        2 Full Charging
        3 Trickle Charging
        4 Waiting
        5 Charging Fault Condition
        */
        char paquete=sensorsGroup3[ChargingState];
        
        paquete = 2;
        byte b = (byte)paquete;
        
        return (int)b;
    }
    /**
     * This code indicates the voltage of Roombaâ€™s battery in millivolts (mV).
     * Range: 0 â€“ 65535 mV
     * @return
     */
    public int voltage(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[Voltage];
        valor[1] = sensorsGroup3[Voltage+1];
        
        valor[0]=2;
        valor[1]=2;
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        //char paquete=sensorsGroup3[Voltage];
        //char paquete2=sensorsGroup3[Voltage+1];
        
        //byte b = (byte)paquete;
        //valor[0]=2;
        //valor[1]=3;
        //int b = (byte)valor[0]+(byte)valor[1];
        
        //System.out.println("El valor es "+Integer.toBinaryString((int)valor[0]));
        //System.out.println("El valor es "+Integer.toBinaryString((int)valor[1]));
        //System.out.println(aux & 0xffff);
        //return (int)b;
        return (aux & 0xffff);
    }
    /**
     * The current in milliamps (mA) flowing into or out of Roombaâ€™s battery. Negative currents indicate that the
     * current is flowing out of the battery, as during normal running. Positive currents indicate that the current
     * is flowing into the battery, as during charging.
     * Range: -32768 â€“ 32767 mA
     * @return
     */
    public int current(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[Current];
        valor[1] = sensorsGroup3[Current+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    /**
     * The temperature of Roombaâ€™s battery in degrees Celsius.
     * Range: -128 â€“ 127
     * @return
     */
    public byte temperature(){
        char paquete=sensorsGroup3[Temperature];
        
        byte b=(byte)paquete;
        
        return b;
    }
    /**
     * The current charge of Roombaâ€™s battery in milliamp-hours (mAh). The charge value decreases as the
     * battery is depleted during running and increases when the battery is charged.
     * Range: 0 â€“ 65535 mAh
     * @return
     */
    public int charge(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[BatteryCharge];
        valor[1] = sensorsGroup3[BatteryCharge+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    /**
     * The estimated charge capacity of Roombaâ€™s battery in milliamp-hours (mAh).
     * Range: 0 â€“ 65535 mAh
     * @return
     */
    public int capacity(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[BatteryCapacity];
        valor[1] = sensorsGroup3[BatteryCapacity+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /** Sensores Grupo 4 **/
    
    /**
     * The strength of the wall signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-1023.
     * @return 
     */
    public int wallSignal(){
        char[] valor = new char[2];
        valor[0] = sensorsGroup4[WallSignal];
        valor[1] = sensorsGroup4[WallSignal+1];

        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    
    /**
     * The strength of the cliff left signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int cliffLeftSignal(){
        char[] valor = new char[2];
        valor[0] = sensorsGroup4[CliffLeftSignal];
        valor[1] = sensorsGroup4[CliffLeftSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /**
     * The strength of the cliff front left signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int cliffFrontLeftSignal(){
        char[] valor = new char[2];
        valor[0] = sensorsGroup4[CliffFrontLeftSignal];
        valor[1] = sensorsGroup4[CliffFrontLeftSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /**
     * The strength of the cliff front right signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int cliffFrontRightSignal(){
        char[] valor = new char[2];
        valor[0] = sensorsGroup4[CliffFrontRightSignal];
        valor[1] = sensorsGroup4[CliffFrontRightSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /**
     * The strength of the cliff right signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int cliffRightSignal(){
        char[] valor = new char[2];
        valor[0] = sensorsGroup4[CliffRightSignal];
        valor[1] = sensorsGroup4[CliffRightSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /**
     * Roombaâ€™s connection to the Home Base and Internal Charger are returned as individual bits, as below.
     * Range: 0-3
     * 1 = charging source present and powered; 0 = charging source not present or not powered.
     * @return
     */
    public int chargerAvailable(){
        char paquete=sensorsGroup4[ChargerAvailable];
        
        /*
         * 1 = charging source present and powered; 0 = charging source not present or not powered.
         */
        int chargingPresent = 1;
        int chargingNotPresent = 0;
        
        //Valor resultado. 666 por defecto
        int resul = 666; 
        
        if((paquete&0x00)!=0){
            if ((paquete&0x01)==1){
                
                //Es 1
                resul = chargingPresent;
            }
        }
        else{
            //Es 0
            resul = chargingNotPresent;
        }
        
        return resul;
    }
    
    /** Sensores Grupo 5 */
    /**
     * The current OI mode is returned. See table below.
     * Range: 0-3
     * Number    Mode
     *   0       Off
     *   1      Passive
     *   2       Safe
     *   3       Full
     * @return 
     */
    public char getOIM(){
        char paquete=sensorsGroup2[OpenInterfaceMode];
        
        return paquete;
    }
    
    /** Sensores Grupo 106 */
    /**
     * The strength of the light bump left signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return 
     */
    public int lightBumpLeftSignal(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[LightBumpLeftSignal];
        valor[1] = sensorsGroup3[LightBumpLeftSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    /**
     * The strength of the light bump front left signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int lightBumpFrontLeftSignal(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[LightBumpFrontLeftSignal];
        valor[1] = sensorsGroup3[LightBumpFrontLeftSignal+1];
        
        //valor[0]=2;
        //valor[1]=2;
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    /**
     * The strength of the light bump front right signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int lightBumpFrontRightSignal(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[LightBumpFrontRightSignal];
        valor[1] = sensorsGroup3[LightBumpFrontRightSignal+1];
        
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    /**
     * The strength of the light bump right signal is returned as an unsigned 16-bit value, high byte first.
     * Range: 0-4095
     * @return
     */
    public int lightBumpRightSignal(){
        char[] valor = new char[2];
        
        valor[0] = sensorsGroup3[LightBumpRightSignal];
        valor[1] = sensorsGroup3[LightBumpRightSignal+1];
        
        short aux1 = (short) (valor[0] & 0xff);
        short aux2 = (short) (valor[1] & 0xff);
        
        int aux = (int)((aux2 << 8) | aux1);
        
        return (aux & 0xffff);
    }
    
    /////////////////////////FIN SENSORES
    
    ///Update Sensors
    
 
    // Fin parte especifica updateSensors()

    // AÃ±adir al constructor de la clase en la versiÃ³n conjunta
	
     
    
    
    /**
     * Metodo para activar los leeds
     * @param checkRobot Led rojo de aviso
     * @param dock Led verde "DOCK"
     * @param spot Led verde "SPOT"
     * @param debris Led azul suciedad
     * @param power_color Color del botÃ³n clean. 0 -> Verde 255-> Rojo
     * @param power_intensity Intensidad de luminosidad. 0-> Apagado 255 -> Maximo
     */
    public void setLEDs(boolean checkRobot, boolean dock, boolean spot, boolean debris, int power_color, int power_intensity) {

        // Preparamos lo que enviar
        byte[] datos = new byte[4];

        // Preperamos el byte de los leds.
        int dato_led = 0;

        if (checkRobot) {
            dato_led += 8;
        }

        if (dock) {
            dato_led += 4;
        }

        if (spot) {
            dato_led += 2;
        }

        if (debris) {
            dato_led += 1;
        }

        // Preparamos el envio
        datos[0] = (byte) SEND_LEDS;
        datos[1] = (byte) dato_led;
        datos[2] = (byte) power_color;
        datos[3] = (byte) power_intensity;

        // Enviamos
        send(datos);
    }

    /**
     * Metodo para mostrar mensaje en LEDs
     * @param digit3 Caracter del mÃ¡s a la izquierda
     * @param digit2 Caracter del segundo por la izquierda
     * @param digit1 Caracter del tercero por la izquierda
     * @param digit0 Ultimo caracter desde la izquierda
     */
    public void SetDigitLEDs(char digit3, char digit2, char digit1, char digit0) {
        
        // Preparamos lo que enviar
        byte[] datos = new byte[5];

        datos[0] = (byte) SEND_DIGIT_LEDS;
        datos[1] = (byte) digit3;
        datos[2] = (byte) digit2;
        datos[3] = (byte) digit1;
        datos[4] = (byte) digit0;

        // Enviamos
        send(datos);

    }

    /**
     * Metodo para evitar encuentros en le canal de comunicacion para la 
     * odometria
     * @param datos Datos a enviar
     * @param tam TamaÃ±o de la informacion a recibir
     * @return Informacion recibida.
     */
    private byte[] OdometryProcessingSynchronized(byte[] datos, int tam) {
        
        // Creamos el hilo
        HiloSensores hilo = new HiloSensores(this, datos, tam);
        byte respuesta[];
        
        // Lo iniciamos y esperamos valores
        if (hilo != null) {
            
            synchronized(synchro)
            {
                hilo.start();
                try {
                    hilo.join();
                } catch (InterruptedException ex) {
                }
                 respuesta = this.datosOdometria;   
            }
            return respuesta;
        } else {
            return null;
        }
    }

    /**
     * Metodo para actualizar los datos de un paquete de sensores en un momento
     * determinado
     * @param packet Identificador del paquete
     * @return Si es todo correcto, true.
     */
    public boolean updateSensors(int packet) {
        // Creamos el hilo
        HiloSensores hilo = new HiloSensores(this, packet);
        
        // Esperamos por los datos
        if (hilo != null) {
            hilo.start();
            try {
                hilo.join();
            } catch (InterruptedException ex) {
            }
            return true;
        } else {
            return false;
        }
    }

    /**
     * Metodo para crear hilos de actualizacion automaticos cada x milisegundos
     * @param packet Grupo de sensores a actualizar
     * @param autoUpdateTime Tiempo de actualizacion
     * @return 1 si se ha activado correctamente
     */
    public int addAutoUpdatePacket(int packet, int autoUpdateTime) {
        // Si no hay hilo creado, lo creamos
        if (this.hilos[packet] == null) {
            HiloSensores hilo = new HiloSensores(this, packet, autoUpdateTime);
            if (hilo != null) {
                this.hilos[packet] = hilo;
                return 1;
            } else {
                return 0;
            }
            // Si esta creado cambiamos el tiempo actualizacion
        } else {
            this.hilos[packet].setAutoUpdateTime(autoUpdateTime);
            return 1;
        }
    }

    /**
     * Metodo para quitar un auto actualizar de sensores
     * @param packet Actualizar que hay que eliminar
     * @return Si se ha eliminado correctamente
     */
    public int removeAutoUpdatePacket(int packet) {
        // Comprobamos si hay alguno y lo quitamos si es necesario
        if (this.hilos[packet] != null) {
            this.hilos[packet].remove();
            this.hilos[packet] = null;
            return 1;
        } else {
            return 0;
        }
    }

    /**
     * Metodo para quitar todos los auto actualizadores de sensores
     */
    public void removeAutoUpdate() {
        for (int i = 0; i < hilos.length; i++) {
            if (hilos[i] != null) {
                if (hilos[i].isAlive()) hilos[i].remove();
                hilos[i] = null;
            }
        }
    }

    /**
     * Metodo para iniciar todos los auto actualizadores de sensores
     */
    public void startAutoUpdate() {
        for (int i = 0; i < hilos.length; i++) {
            if (hilos[i] != null && !(hilos[i].isAlive())) {
                hilos[i].start();
            }
        }
    }

    /**
     * Metodo para obtener una cadena con el estado de los sensores.
     * @return String con representaciÃ³n del estado de los sensores
     */
    public String sensorsAsString(){
        
       
        return
            "bumper:" + (bump()?("s" + (bumpLeft()?"l":"") + (bumpRight()?"r":"")):"n")
             +
            " wheel:" +
            (wheelDropLeft()  ?"l":"_") +
            (wheelDropLeft()  ?"r":"_") +
            " wall:" + (wall() ?"Y":"n") + 
            " cliff:" +
            (cliffLeft()       ?"l":"_") +
            (cliffFrontLeft()  ?"L":"_") +  
            (cliffFrontRight() ?"R":"_") +
            (cliffRight()      ?"r":"_") +
            " dirt:"+ dirt()+
            " vwal:" + (virtualWall()?"Y":"n") +
            " motr:" + 
                "DL" + (motorOvercurrentDriveLeft()?"Y":"n") +
                "DR" + (motorOvercurrentDriveRight()?"Y":"n") +
                "B" + (motorOvercurrentMainBrush()?"Y":"n") +
                "b" + (motorOvercurrentSideBrush()?"Y":"n") +
                "V" + (motorOvercurrentVacuum()?"Y":"n") +
            " butt:"
                + "cl" +   (clockButton()?"Y":"n") +
                "sch" + (scheduleButton()?"Y":"n") +
                "day" + (dayButton()?"Y":"n") +
                "h" + (hourButton()?"Y":"n") +
                "m" + (minuteButton()?"Y":"n") +
                "d" + (dockButton()?"Y":"n") +
                "p"  + (spotButton()?"Y":"n") +
                "c" +    (cleanButton()?"Y":"n") +
            " chst:" + chargingState() + 
            " volt:" + voltage() +
            " curr:" + current() +
            " temp:" + temperature() +
            " chrg:" + charge() +
            " capa:" + capacity();          
    }
    
    /**
     * Metodo para hacer una pausa x milisegundos
     * @param millis Pausa deseada en milisegundos
     */
    protected void pause(int millis) {
        try {
            Thread.sleep(millis);
        } catch (Exception e) {
        }
    }

    /**
     * Metodo para comprobar que los movimientos sean seguros
     * @return Si un movimiento es seguro
     */
    private boolean computeSafetyFault(){
        if(wheelDropLeft()|| wheelDropRight()|| cliffFrontLeft() || cliffFrontRight() || cliffLeft() || cliffRight()){
            return false;
        }else{
            return true;
        }
    }
    
    
    /**
     * Metodo para cambiar el estado de los arrays de estado
     * @param packet Grupo de sensores que cambiar
     * @param recibido Datos nuevos
     */
    protected void setSensorsGroup(int packet, byte[] recibido) {

        if (packet != ODOMETRIA) {
            char[] datos = new char[recibido.length];

            for (int i = 0; i < datos.length; i++) {
                datos[i] = (char) recibido[i];
            }

            switch (packet) {
                case GRUPO_SENSORES_1:
                    this.sensorsGroup1 = datos;
                    break;
                case GRUPO_SENSORES_2:
                    this.sensorsGroup2 = datos;
                    break;
                case GRUPO_SENSORES_3:
                    this.sensorsGroup3 = datos;
                    break;
                case GRUPO_SENSORES_4:
                    this.sensorsGroup4 = datos;
                    break;
                case GRUPO_SENSORES_5:
                    this.sensorsGroup5 = datos;
                    break;
                case GRUPO_SENSORES_106:
                    this.sensorsGroup106 = datos;
                    break;
            }
        } else {
            this.datosOdometria = recibido;
        }
    }
    
    //////////////////////FIN UPDATE SENSORS
    


    // FunciÃ³n de mÃ©todos auxiliares, coger de esa parte
    /*boolean send(byte[] bytes) {
        return true; // Por poner algo
    }*/
    

    // Funciones de la API de Rombacomm, coger de la API
    public String hex(byte b) {
        return Integer.toHexString(b & 0xff);
    }

    public void logmsg(String msg, boolean debug) {
        if (debug) {
            System.err.println("RoombaComm (" + System.currentTimeMillis() + "):" + msg);
        }
    }




    /********************************************************************/
    /************************ COMANDOS DE ACCION ************************/
    /********************************************************************/

    /*
     * Get speed for movement commands
     * 
     * @return speed Value of Roomba speed
     */
    public int getSpeed() {
        return speed;
    }

    /**
     * Set speed for movement commands
     * 
     * @param speed 
     */
    public void setSpeed(int speed) {
        this.speed = speed;
    }

    /**
     * Move the Roomba via the low-level velocity + radius method. See the
     * 'Drive' section of the Roomba ROI spec for more details. Low-level
     * command.
     * 
     * @param velocity speed in millimeters/second, positive forward, negative
     * backward
     * @param radius radius of turn in millimeters
     */
    public void drive(int velocity, int radius) 
    {
        byte cmd[] = {(byte) DRIVE, // [137]
                      (byte) (velocity >>> 8), // [Velocity high byte]
                      (byte) (velocity & 0xff), // [Velocity low byte]
                      (byte) (radius >>> 8), // [Radius high byte]
                      (byte) (radius & 0xff) // [Radius low byte]
        };
        
        logmsg("drive: " + hex(cmd[0]) + "," + hex(cmd[1]) + ","
                       + hex(cmd[2]) + "," + hex(cmd[3]) + "," + hex(cmd[4]),true);
        
        send(cmd);
    }

    /**
     * Control the forward and backward motion of Roombaâ€™s drive wheels
     * independently. Velocity in mm/s
     *
     * @param rightVelocity speed in millimeters/second of the right wheel
     * positive forward, negative backward
     * @param leftVelocity speed in millimeters/second of the left wheel
     * positive forward, negative backward
     */
    public void driveDirect(int rightVelocity, int leftVelocity) {
        byte cmd[] = {(byte) DRIVEWHEELS, // [145]
            (byte) (rightVelocity >>> 8), // [Right velocity high byte]
            (byte) (rightVelocity & 0xff), // [Right velocity low byte]
            (byte) (leftVelocity >>> 8), // [Left velocity high byte]
            (byte) (leftVelocity & 0xff) // [Left velocity low byte]
        };
        logmsg("drive direct: " + hex(cmd[0]) + "," + hex(cmd[1]) + ","
                + hex(cmd[2]) + "," + hex(cmd[3]) + "," + hex(cmd[4]),true);
        send(cmd);
    }


    /********* BACKWARD *********/

    /**
     * Go backward at the current (negative) speed
     */
    public void goBackward() {
        goStraightAt(-Math.abs(this.getSpeed()));
    }

    /**
     * Go backward during a specified distance, at the current (negative) speed
     * 
     * @param distance distance in millimeters, positive
     */
    public void goBackward(int distance) throws InterruptedException {
        if (distance < 0) {
            return;
        }
        goStraight(-distance);
    }

    /**
     * Go backward at a specified speed
     * 
     * @param aspeed speed in millimeters/second
     */
    public void goBackwardAt(int aspeed) {
        if (aspeed < 0) {
            return;
        }
        goStraightAt(-aspeed);
    }

    /**
     * Go backward at a specified speed
     * 
     * @param distance distance in millimeters, positive
     * @param aspeed speed in millimeters/second
     * @throws InterruptedException
     */
    public void goBackwardAt(int distance, int aspeed) throws InterruptedException {
        if (distance < 0) {
            return;
        }
        if (aspeed < 0) {
            return;
        }
        goStraightAt(-distance, -aspeed);
    }

    
    /********* FORWARD *********/

    /**
     * Go forward the current (positive) speed
     */
    public void goForward() 
    {
        goStraightAt(Math.abs(speed));
    }

    /**
     * Go forward during a specified distance, at the current (positive) speed
     * 
     * @param distance distance in millimeters, positive
     */
    public void goForward(int distance) throws InterruptedException {
        if (distance < 0) {
            return;
        }
        goStraight(distance);
    }

    /**
     * Go forward at a specified speed
     * 
     * @param aspeed speed in millimeters/second
     */
    public void goForwardAt(int aspeed) {
        if (aspeed < 0) {
            return;
        }
        goStraightAt(aspeed);
    }

    /**
     * Go forward at a specified speed
     * 
     * @param distance distance in millimeters, positive
     * @param aspeed speed in millimeters/second
     * @throws InterruptedException 
     */
    public void goForwardAt(int distance, int aspeed) throws InterruptedException {
        if (distance < 0) {
            return;
        }
        if (aspeed < 0) {
            return;
        }
        goStraightAt(distance, aspeed);
    }


    /********* STRAIGHT *********/

    /**
     * Go straight at the current speed for a specified distance
     * 
     * @param distance distance in millimeters
     * @throws InterruptedException 
     */
    public void goStraight(int distance) throws InterruptedException 
    {
        float thetaInicial = this.getTheta();
        float xInicial     = this.getX();
        float yInicial     = this.getY();
        float thetaTemp    = thetaInicial;
        float xTemp        = xInicial;
        float yTemp        = yInicial;

        if (distance > 0) 
        {
            goStraightAt(this.getSpeed());
        } 
        else 
        {
            goStraightAt(-this.getSpeed());
        }

        while (Math.abs(distance) < calcularLongitud(xInicial, xTemp, yInicial, yTemp)) 
        {
            xTemp = this.getX();
            yTemp = this.getY();
            // Se duerme durante el tiempo de refresco de los valores de odometrÃ­a
            // para dar tiempo para actuailizar los valores x, y, theta
            Thread.sleep(getOdometryUpdateTime());

            // Ahora se comprueba si el Roomba estÃ¡ siguiendo una lÃ­nea recta
            thetaTemp = this.getTheta();
            // No sigue la lÃ­nea recta
            if (thetaInicial != thetaTemp) {
                if (thetaTemp > thetaInicial) { // Se estÃ¡ girando a la izquierda
                    if (distance > 0) {
                        corregirDireccion(this.getSpeed(), thetaInicial-thetaTemp, 
                                Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    } else {
                        corregirDireccion(-this.getSpeed(), thetaInicial-thetaTemp, 
                                Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    }
                } else { // Se estÃ¡ girando hacia la derecha
                    if (distance > 0) {
                        corregirDireccion(this.getSpeed(), thetaInicial-thetaTemp, 
                                Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    } else {
                        corregirDireccion(-this.getSpeed(), thetaInicial-thetaTemp, 
                                Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    }
                }
                // Se ha recuperado la lÃ­nea recta o se sigue en ella, asi que se vuelve a ir recto
            } else {
                if (distance > 0) {
                    goStraightAt(this.getSpeed());
                } else {
                    goStraightAt(-this.getSpeed());
                }
            }
        }
        stop();
    }

    /**
     * FunciÃ³n para calcular la longitud de una recta en funciÃ³n de sus
     * puntos iniciales y finales
     * 
     * @param x1 punto x inicial
     * @param x2 punto x final
     * @param y1 punto y inicial
     * @param y2 punto y final
     * @return longitud de la recta
     */
    private float calcularLongitud(float x1, float x2, float y1, float y2) {
        return (float) Math.sqrt(Math.pow((x2 - x1), 2) + Math.pow((y2 - y1), 2));
    }

    /**
     * FunciÃ³n similar a Straight pero a la que se le puede indicar el radio
     * 
     * @param velocity en milÃ­metros por segundo
     * @param diferencia angulo en milÃ­metros desviado
     * @param distancia distancia que queda para el final
     */
    public void corregirDireccion(int velocity, float diferencia, float distancia) {
        int radius = 0x8000; // Por defecto que siga recto

        if (velocity > 500) {
            velocity = 500;
        }
        if (velocity < -500) {
            velocity = -500;
        }

        // Se calcula el radio de giro que debe llevar para corregir la direcciÃ³n
        if (diferencia > 0) {
            // TODO
        } else {
            // TODO
        }
        
        drive(velocity, radius);
    }

    /**
     * Go straight at a specified speed
     * 
     * @param velocity speed in millimeters/second
     */
    public void goStraightAt(int velocity) 
    {
        System.out.println(velocity);
        //System.out.println("goStraightAt: velocity:"+velocity);
        if (velocity > 500) 
        {
            velocity = 500;
        }
        if (velocity < -500) 
        {
            velocity = -500;
        }
        drive(velocity, 0x8000);
    }

    /**
     * Go straight at a specified speed
     * 
     * @param distance distance in millimeters
     * @param velocity speed in millimeters/second, positive
     * @throws InterruptedException 
     */
    public void goStraightAt(int distance, int velocity) throws InterruptedException {
        float thetaInicial = this.getTheta();
        float xInicial     = this.getX();
        float yInicial     = this.getY();
        float thetaTemp    = thetaInicial;
        float xTemp        = xInicial;
        float yTemp        = yInicial;

        if (distance > 0) {
            goStraightAt(velocity);
        } else {
            goStraightAt(-velocity);
        }

        while (Math.abs(distance) < calcularLongitud(xInicial, xTemp, yInicial, yTemp)) {
            xTemp = this.getX();
            yTemp = this.getY();
            // Se duerme durante el tiempo de refresco de los valores de odometrÃ­a
            // para dar tiempo para actuailizar los valores x, y, theta
            Thread.sleep(getOdometryUpdateTime());

            // Ahora se comprueba si el Roomba estÃ¡ siguiendo una lÃ­nea recta
            thetaTemp = this.getTheta();
            // No sigue la lÃ­nea recta
            if (thetaInicial != thetaTemp) {
                if (thetaTemp > thetaInicial) { // Se estÃ¡ girando a la izquierda
                    if (distance > 0) {
                        corregirDireccion(velocity, thetaInicial-thetaTemp, Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    } else {
                        corregirDireccion(-velocity, thetaInicial-thetaTemp, Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    }
                } else { // Se estÃ¡ girando hacia la derecha
                    if (distance > 0) {
                        corregirDireccion(velocity, thetaInicial-thetaTemp, Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    } else {
                        corregirDireccion(-velocity, thetaInicial-thetaTemp, Math.abs(distance) - calcularLongitud(xInicial, xTemp, yInicial, yTemp));
                    }
                }
                // Se ha recuperado la lÃ­nea recta o se sigue en ella, asi que se vuelve a ir recto
            } else {
                if (distance > 0) {
                    goStraightAt(velocity);
                } else {
                    goStraightAt(-velocity);
                }
            }
        }
        stop();
    }


    /********* SPIN *********/

    /**
     * Spin right or spin left a particular number of degrees
     *
     * @param angle angle in degrees, positive to spin left, negative to spin
     * right
     */
    public void spin(int angle) throws InterruptedException {
        if (angle > 0) {
            spinLeft(angle);
        } else if (angle < 0) {
            spinRight(-angle);
        }
    }

    /**
     * Spin in place anti-clockwise, at the current speed
     */
    public void spinLeft() {
        spinLeftAt(this.getSpeed());
    }

    /**
     * Spin left a specified angle at a specified speed
     *
     * @param angle angle in degrees, positive
     * @throws InterruptedException
     */
    public void spinLeft(int angle) throws InterruptedException {
        if (angle < 0) {
            return;
        }
        int resto          = 0;
        float thetaIniclal = this.getTheta();
        int numVueltas     = angle / 360;
        resto              = angle % 360;

        spinLeftAt(Math.abs(this.getSpeed()));
        for (int i = 0; i < numVueltas; i++) {
            while (this.getTheta() != thetaIniclal + 1) {
                Thread.sleep(getOdometryUpdateTime());
            }
        }

        while (this.getTheta() != (thetaIniclal + resto - 1)) {
            Thread.sleep(getOdometryUpdateTime());
        }

        stop();
    }

    /**
     * Spin in place anti-clockwise, at the current speed.
     *
     * @param aspeed speed to spin at
     */
    public void spinLeftAt(int aspeed) {
        drive(aspeed, 1);
    }

    /**
     * Spin in place anti-clockwise, at a specified speed
     * 
     * @param angle angle in degrees, positive
     * @param aspeed speed to spin at
     * @throws InterruptedException 
     */
    public void spinLeftAt(int angle, int aspeed) throws InterruptedException {
        if (angle < 0) {
            return;
        }
        int resto;
        float thetaIniclal = this.getTheta();
        int numVueltas = angle / 360;
        resto = angle % 360;
        spinLeftAt(Math.abs(aspeed));
        for (int i = 0; i < numVueltas; i++) {
            while (this.getTheta() != thetaIniclal + 1) {
                Thread.sleep(getOdometryUpdateTime());
            }
        }
        while (this.getTheta() != (thetaIniclal + resto - 1)) {
            Thread.sleep(getOdometryUpdateTime());
        }
        stop();
    }

    /**
     * Spin in place clockwise, at the current speed
     */
    public void spinRight() {
        spinRightAt(speed);
    }

    /**
     * Spin right the current speed for a specified angle
     *
     * @param angle angle in degrees, positive
     * @throws InterruptedException
     */
    public void spinRight(int angle) throws InterruptedException {
        if (angle < 0) {
            return;
        }
        int resto;
        float thetaIniclal = this.getTheta();
        int numVueltas = angle / 360;

        resto = angle % 360;
        spinRightAt(Math.abs(this.getSpeed()));
        for (int i = 0; i < numVueltas; i++) {
            while (this.getTheta() != thetaIniclal - 1) {
                Thread.sleep(getOdometryUpdateTime());
            }
        }
        while (this.getTheta() != (thetaIniclal + resto + 1)) {
            Thread.sleep(getOdometryUpdateTime());
        }
        stop();
    }

    /**
     * Spin in place clockwise, at the current speed.
     *
     * @param aspeed speed to spin at, positive
     */
    public void spinRightAt(int aspeed) {
        drive(aspeed, -1);
    }

    /**
     * Spin in place clockwise, at the current speed.
     * 
     * @param angle 
     * @param aspeed
     * @throws InterruptedException 
     */
    public void spinRightAt(int angle, int aspeed) throws InterruptedException {
        if (angle < 0) {
            return;
        }
        int resto;
        float thetaIniclal = this.getTheta();
        int numVueltas = angle / 360;
        resto = angle % 360;
        spinRightAt(Math.abs(aspeed));
        for (int i = 0; i < numVueltas; i++) {
            while (this.getTheta() != thetaIniclal - 1) {
                Thread.sleep(getOdometryUpdateTime());
            }
        }
        while (this.getTheta() != (thetaIniclal + resto + 1)) {
            Thread.sleep(getOdometryUpdateTime());
        }
        stop();
    }


    /********* OTHERS *********/

    /**
     * Stop Rooomba's motion. Sends drive(0,0)
     */
    public void stop() {
        logmsg("stop",true);
        drive(0, 0);
    }

    /**
     * Turn all vacuum motors on or off according to state
     *
     * @param state true to turn on vacuum function, false to turn it off
     */
    public void vacuum(boolean state) {
        //logmsg("vacuum: " + state);
        setMotors(state, state, state);
    }

    /**
     * Turns on/off the non-drive motors (main brush, vacuum, sidebrush). Sort
     * of low-level.
     *
     * @param mainbrush mainbrush motor on/off state
     * @param vacuum vacuum motor on/off state
     * @param sidebrush sidebrush motor on/off state
     */
    public void setMotors(boolean mainbrush, boolean vacuum, boolean sidebrush) {
        byte cmd[] = {
            (byte) MOTORS,
            (byte) ((mainbrush ? 0x04 : 0) | (vacuum ? 0x02 : 0) | (sidebrush ? 0x01 : 0))};
        send(cmd);
    }
    
    ///////////COMANDOS ACCION
    


    
    byte[] odometryProcessingSynchronized(){
        byte sensores[] = new byte[8];
        sensores[0] = 0;
        sensores[1] = 0;
        sensores[2] = 0;
        sensores[3] = 0;
        sensores[4] = 0;
        sensores[5] = 0;
        sensores[6] = 0;
        sensores[7] = 0;
        
        return this.sensores;
    }
    
    void setOdometryInitialPosition(float x, float y, float Theta){
        this.x = x;
        this.y = y;
        this.Theta = Theta;
    }
    
    void startOdometryProcessing(){
        odometria = new HiloOdometria(this);
        hiloOdometria = new Thread(odometria);
        hiloOdometria.start();
    }
    
    void stopOdometryProcessing(){
        odometria.stop();
    }
    
    void setOdometryUpdateTime(int odometryUpdateTime){
        if(odometryUpdateTime > 0) this.odometryUpdateTime = odometryUpdateTime;
    }
    
    int getOdometryUpdateTime(){
        if(this.odometryUpdateTime <= 0) return RoombaUSAL.defaultOdometryUpdateTime;
        return this.odometryUpdateTime;
    }
    
    public float getX(){
        return this.x;
    }
    
    public float getY(){
        return this.y;
    }
    
    public float getTheta(){
        return this.Theta;
    }
    
    public float getRightEncoderConversion(){
        return this.rightEncoderConversion;
    }
    
    public float getLeftEncoderConversion(){
        return this.leftEncoderConversion;
    }
    
    public void setX(float nuevoX){
        this.x = nuevoX;
    }
    
    public void setY(float nuevoY){
        this.y = nuevoY;
    }
    
    public void setLeftEncoderConversion(float nuevo){
        this.leftEncoderConversion = nuevo;
    }
    
    public void setRightEncoderConversion(float nuevo){
        this.rightEncoderConversion = nuevo;
    }
    
    public void setTheta(float nuevoTheta){
        this.Theta = nuevoTheta;
    }
    
    public boolean getDebug(){
        return this.debug;
    }
    
    public void setDebug(boolean debug){
        this.debug = debug;
    }
    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) throws IOException {
        // TODO code application logic here
        RoombaUSAL roomba = new RoombaUSAL(true, RoombaUSAL.RoombaSerialConfig.SPEED[0]);
        roomba.connect("COM5");
        roomba.start();
        
        byte cmd[] = {(byte) DRIVE, // [137]
                      (byte) (255), // [Velocity high byte]
                      (byte) (56), // [Velocity low byte]
                      (byte) (1), // [Radius high byte]
                      (byte) (244) // [Radius low byte]
        };
        
        roomba.send(137);
        
        System.out.println("sdfsdf");
        roomba.goForward();
        roomba.startAutoUpdate();
        while(true){
          
        System.out.println(roomba.sensorsAsString());
        
        }
           
        
        
            //roomba.setOdometryUpdateTime(1000);
            //roomba.startOdometryProcessing();
        
    }
    
    
    
}
